#include "lpc13xx.h"
#include "gpio.h"
#include "vs-wrc103.h"
#include "ixbus.h"
#include "HMC6352.h"


/*マクロ***********************************************************/

/*グローバル変数***********************************************************/

/*メイン関数***********************************************************/
int main (void)
{
	// モーター制御ゲイン。
	const double GainK = 5.0;

	// シリアル(UART)通信テスト用。
	unsigned char test = 0;

	//シリアル(UART)通信初期化
	InitSci3(CBR_115200,non,1);

	//制御周期の設定[単位：Hz　範囲：30.0~]
	const unsigned short MainCycle = 60;
	Init(MainCycle);		//CPUの初期設定
	I2C_init();				//IXBUS初期化

	// エンコーダの初期値。
	InitEncoder();	//エンコーダを初期化します。使用する前にからなず読みだす必要があります。
	ClearEncoder();	//エンコーダの値をクリアします。

	LED(3);

	// スタート待機。
	while(getSW() != 1);	//ボタンが押されるまで待つ
	while(getSW() == 1);	//ボタンが離されるまで待つ

	//ループ
	while(1){
		long R_enc_cnt = 0, L_enc_cnt = 0;	// エンコーダの値を格納するための変数
		long R_speed = 0, L_speed = 0;		// スピード値。
		static long target_R_spd = 0, target_L_spd = 0;	// 目標スピード値。
		static long old_R_enc_cnt = 0, old_L_enc_cnt = 0;	// 前回エンコーダ値。
		static short R_motor_out = 0, L_motor_out = 0;		// モータ出力値。
		static short status = 0;	// 動作ステータスフラグ。
		unsigned short compass = 0;	// コンパス値。

		// 同期関数。設定した周期（ここでは60Hz）毎に、次の処理を実行するために使用。
		Sync();

		// エンコーダ値を取得。
		GetEncoder(&R_enc_cnt, &L_enc_cnt);

		// エンコーダ値と前回エンコーダ値からスピード算出。
		R_speed = (double)(R_enc_cnt - old_R_enc_cnt) * 0.173 * 60.0;
		L_speed = (double)(L_enc_cnt - old_L_enc_cnt) * 0.173 * 60.0;

		// 目標値 60mm/sec との差にゲインを積算して加速分をモータ出力に加算。
		R_motor_out += (short)(GainK * (target_R_spd - R_speed));
		L_motor_out += (short)(GainK * (target_L_spd - L_speed));

		// モータに速度を設定。
		Mtr_Run_lv(R_motor_out, L_motor_out, 0, 0, 0, 0);

		// エンコーダは 0.173[mm/カウント]=5780[カウント/m]。
		// 車体のトレッド（車輪間距離）は100mm。

		// 起動直後（status = 0)のとき、右回転（真地旋回）を開始させる。
		if (status == 0)
		{
			// エンコーダカウントクリア。
			ClearEncoder();
			R_enc_cnt = 0;
			L_enc_cnt = 0;
			// 右輪を逆転(CCW)=後退に設定。
			target_R_spd = -40;
			// 左輪を逆転=前進に設定。
			target_L_spd = -40;

			// コンパスにキャリブレーション開始コマンドを送信する。
			startCalibration_hmc6352();

			// ステータス更新。
			status++;
		}
		// status = 1 で右回転中。
		if  (status == 1 && (R_enc_cnt < -3650 || L_enc_cnt < -3650))
		{
			// 2回転したら、右回転終了。
			target_R_spd = 0;
			target_L_spd = 0;

			// コンパスにキャリブレーション終了コマンドを送信する。
			endCalibration_hmc6352();

			// ステータス更新。
			status++;
		}

		// 右回転終了後(status = 2)、磁北を向く動作へ移行。
		if (status == 2)
		{
			// エンコーダカウントクリア。
			ClearEncoder();
			R_enc_cnt = 0;
			L_enc_cnt = 0;

			// ステータス更新。
			status++;
		}
		// status = 3で磁北を向く動作。
		if (status == 3)
		{
			// コンパスの値を取得。
			compass = read_hmc6352();

			if (compass <= 150 || compass >= 3450)
			{
				// コンパス値が3450～3559,0～150に入っていれば、停止。
				target_R_spd = 0;
				target_L_spd = 0;
			}
			else
			{
				if (compass <= 450 )
				{
					// 東（右方向）を向いている場合は左回転。
					target_R_spd = 15;
					target_L_spd = 15;
				}
				if (compass > 450 && compass < 1800)
				{
					// 東（右方向）を向いている場合は左回転。
					target_R_spd = 30;
					target_L_spd = 30;
				}
				if (compass >= 2700)
				{
					// 西（左方向）を向いている場合は右回転。
					target_R_spd = -15;
					target_L_spd = -15;
				}
				if (compass >= 1800 && compass < 2700)
				{
					// 西（左方向）を向いている場合は右回転。
					target_R_spd = -30;
					target_L_spd = -30;
				}
			}
		}

		// エンコーダカウント値を保存。
		old_R_enc_cnt = R_enc_cnt;
		old_L_enc_cnt = L_enc_cnt;

		// シリアル通信テスト。
		if(SciByteRx(&test) > 0){
			LED(2);		//オレンジのLED点灯
			SciByteTx(test);
		}
	}
	Mtr_Run_lv(0,0,0,0,0,0);

	return 0;
}
